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AN  OPTIMAL  DEPTH  CONTROL  TECHNIQUE  FOR  UNDERWATER  VEHICLES 

SECTION  I 
INTRODUCTION 

The  ability  of  a  torpedo  or  a  torpedo-like  vehicle  to  maintain  a 
preset  depth  and  course  is  a  fundamental  prerequisite  for  successful 
operation.  This  is  equally  true  whether  the  vehicle  is  operating  in  a 
calm  sea  devoid  of  all  currents,  or  in  high  sea  state  conditions  near 
the  surface  where  it  would  likely  be  very  much  affec. ed  by  ocean  currents 
and  wave  induced  eddies.  In  addition,  most  torpedo-like  underwater 
vehicles  are  non-neutrally  buoyant  and  have  centers  of  gravity  and 
buoyancy  that  do  not  coincide.  For  such  vehicles  to  maintain  a  constant 
depth,  the  steady  state  values  for  pitch  angle,  angle  of  attack,  and 
elevator  deflection  have  non-zero  values  (reference  1) . 

The  approach  used  here  to  develop  an  optimal  depth  control  law  for 
torpedoes  is  applicable  to  underwater  vehicles  in  general,  and  the 
technique  developed  will  maintain  a  vehicle  at  a  constant  depth  when  it 
operates  under  the  influence  of  seaway  disturbances  and  weight -buoyancy 
imbalances.  The  hydrodynamic  equations  of  motion  for  a  symmetrical  under¬ 
water  vehicle  are  used  in  the  development  of  state  feedback  controllers 
for  such  vehicles.  In  the  past,  development  of  these  closed-loop  con¬ 
trollers  was  accomplished  by  using  either  a  classical  approach  (refer¬ 
ences  2,  3,  4)  or  by  computer  simulations  (references  5-6).  This  report 
analytically  derives  the  feedback  gains  utilizing  modern  control  theory 
and  Luenberger  observer  theory  (reference  7).  Emphasis  £s  focused  on 
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such  factors  as  the  realization  of  a  linear  state  variable  model,  in 
which  the  states  are  completely  controllable  and  observable,  and  a  per¬ 
formance  index  which  weights  the  tradeoffs  between  errors  in  vehicle 
depth  and  expenditure  of  control  effort  to  correct  these  depth  errors. 

In  Section  II  a  linear,  controllable,  and  observable  state  variable 
model  of  the  pitch  plane  dynamics  of  the  vehicle  is  developed  in  terms 
of  the  hydrodynamic  moments  and  forces  given  in  reference  8.  A  differ¬ 
ential  equation  delineating  the  vertical  velocity  of  the  vehicle  is  in¬ 
cluded  in  this  model ,  which  is  for  a  non-neutrally  buoyant  vehicle  with 
different  centers  of  gravity  and  buoyancy,  and  which  takes  into  account 
the  effects  of  ocean  disturbances.  The  state  variables  sufficient  to 
define  this  system  are  vehicle  depth,  pitch,  pitch  rate,  and  angle  of 
attack  (resulting  in  a  fourth-order  system) ,  Three  of  the  state  variables 
are  available  for  measurement  at  the  output,  but  the  fourth  state,  angle 
of  attack,  must  be  estimated  by  using  a  reduced  order  Luenberger  observer. 
In  Section  III,  the  optimal  control  problem  is  formulated  as  an  out¬ 
put  regulator  with  a  constant  reference  input  vector  (reference  1). 

This  approach  determines  an  optimal  control  law  that  keeps  the  output  of 
the  dynamic  system  near  a  constant  reference  while  minimizing  a  quadratic 
cost  functional.  The  quadratic  cost  functional  comprises  a  weighted 
error  state  vector  (the  difference  between  the  output  of  the  dynamic 
model  and  a  desired  constant  input)  and  a  weighted  input  state  vector. 
Minimization  of  the  quadratic  cost  functional  results  in  an  optimal 
control  law,  which  drives  the  output  to  the  desired  reference  while  min- 
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imizing  control  expended.  The  resulting  control  law  is  the  product  of 
the  states  of  the  system  and  a  constant  gain  matrix,  which  is  a  function 
of  the  weighting  matrices  in  the  cost  functional  and  the  linear  state 
variable  model.  The  development  of  a  reduced-order  Luenberger  observer 
to  obtain  an  estimate  of  the  angle  of  attack  is  included. 

The  hydrodynamic  coefficients  of  a  typical  torpedo  traveling  at  a 
constant  speed  are  used  in  the  state  variable  model  to  illustrate  the 
feasibility  of  the  control  scheme.  The  model  was  simulated  on  a  digital 
computer  and  the  stability  of  the  vehicle  is  analyzed  for  different  per¬ 
formance  index  weighting  matrices  and  vehicle  speeds.  The  optimal  con¬ 
trol  law  was  formulated  for  a  linear  state  variable  model,  but  limita¬ 
tions  were  placed  on  both  the  vehicle's  pitch  and  rudder  deflection  in 
the  simulation.  By  adjusting  the  weighting  matrices  in  the  performance 
index,  a  good  solution  to  the  nonlinear  problem  can  be  obtained.  Also, 
since  the  closed-loop  transfer  function  is  stable,  the  control  scheme 
will  enter  the  linear  range.  Calcomp  plots  of  the  states  and  control 
inputs  were  made  to  aid  in  the  analysis  of  the  optimal  controller  devel¬ 
oped  herein.  These  are  presented  and  discussed  in  Section  IV. 
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SECTION  II 

DEVELOPMENT  OF  THE  STATE  VARIABLE  MODEL 

From  the  hydrodynamic  equations  of  motion  of  an  underwater  vehicle, 
a  linear,  controllable,  and  observable  state  variable  model  is  developed 
that  takes  into  account  the  effects  of  seaway  disturbances.  This  model 
is  simplified  by  assuming  that  the  vehicle  is  neutrally  buoyant  with 
centers  of  gravity  and  buoyancy  that  coincide,  and  also  that  the  vehicle 
is  not  affected  by  seaway  disturbances.  This  simplified  model  is  used 
to  develop  the  initial  optimal  control  law  presented  in  the  next  section. 

LINEAR  PITCH  PLANE  MODEL 

The  equations  of  motion  for  a  submerged  hydrodynamic  body  in  the 
yaw,  pitch,  and  roll  planes  are  given  in  reference  8.  To  simplify  these 
equations,  the  following  assumptions  are  made  (from  reference  2). 

1.  Pitch,  yaw,  and  angle  of  attack  satisfy  the  small  angle 
approximations, 

2.  Vehicle  is  traveling  at  a  constant  speed. 

3.  Roll  motion  is  constrained  to  negligibly  small  values  by  the 
roll  control  system. 

Under  these  conditions,  the  roll,  yaw,  and  pitch  dynamics  are 
mutually  decoupled.  This  results  in  a  set  of  linear  equations  that 
represents  the  force  and  moment  summations.  Figure  1  depicts  a  sub¬ 
merged  vehicle  in  the  pitch  plane. 
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Figure  1.  Submerged  Vehicle  in  the  Pitch  Plane 
The  linear  pitch  plane  equations  are 

tatVu  *  mXg3  +  (Zq+mLV)e  +  Zaa  +  Z5e<5e  +  (W-B)  *  (1) 

and 

Jy9  -  mXgVi  +  Maa  +  (Mq-mXgVie  +  MSe5e  -  WXg  (2) 

where 

a  =  angle  of  attack 

9  =  pitch  angle 

5e  =  elevator  deflection 

and  the  vehicle  constants  and  the  hydrodynamic  coefficients  are  defined 
in  appendix  A. 

*  (')  indicates  ^  (  )  and  (”)  indicates  ~_2(  ) 
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The  depth  equation  is  given  by 

20  *  -Vsin(9-a)  *  -V(9-a)  (3) 

where 

Z0  3  depth  of  vehicle  below  the  mean  surface. 

STATE  VARIABLE  REPRESENTATION 

Equations  (1),  (2),  and  (3)  are  put  into  a  state  variable  notation 
by  letting 

Xi(t)  -  z0 

X2(t)  *  e  (4) 

X3(t)  -  9  -  X^t) 

X4(t)  -  a 
and  y(t)  =  6e 

In  terms  of  the  state  variables  defined,  the  state  equations  are 
written  directly  from  equations  (1),  (2),  and  (3)  as 

otVX4(t)  -  mXgX3(t)  +  (Zq+injV^ (t)  +  ZaX4(t)  +  Z5{,U(t)  +  (W-B)  (5) 

JyX3(t)  -  mXgVX4(t)  +  MaX4(t)  +  (Mq-mXgV)X3(t)  +  M5eU(t)  -  WXg  (6) 
xx(t)  -  -VX2(t)  +  vx4(t).  (7) 
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•  * 

Solving  for  the  states  X^(t)  and  X^(t)  in  equations  (5)  and  (6)  and 
using  equations  (4)  and  (7)  results  in  a  state  variable  representation 


of  the  form 

XL(t)  -  -vx2(t)  +  vx4(t)  (8a) 

X2(t)  -  X3(t)  (8b) 

X3(t)  -  X3(t)  +  a,  X4(t)  +  b1  U(t)  +  b3  (8C) 

X4(t)  -  a3  X3(t)  +  a4  X4(t)  +  b2  U(t)  +  b4  (8d) 

where 

,  [mXg(Zq-hnLV)+(Mq-mXgV)mt] 


a2  * 


[mXgZa+Mamc] 

B 


(8f) 


JyfZq-HujV]  +  mXgtMq-mXgV] 

VS 


(8g) 


-  ,  Jy  Za  +  mX^ 

4  . 

VS 


(8h) 


Ei-  (81) 

S 
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F2  -  Jy  Z,$e  +  aXgMge 
VS 


(8j) 


f3 


[mXg(W-B)  -  WXgmt] 
8 


(8k) 


b,  -  Jy(W-B)  -  mXgW  (81) 

V3 


and 

S  -  Jymj.  -  m2X2.  (8m> 

SEAWAY  DISTURBANCE  ON  UNDERWATER  VEHICLE  OPERATION 

When  an  underwater  vehicle  operates  near  the  surface  in  a  high 
sea  state  condition,  ocean  currents  can  cause  the  vehicle  to  deviate 
from  the  desired  trajectory.  To  include  the  effect  of  such  seaway 
disturbances,  the  following  assumptions  are  made  (reference  2) . 

1.  The  dynamic  response  to  control  surface  deflection  is  indepen¬ 
dent  of  the  disturbance  effects. 

2.  The  horizontal  speed,  in  relation  to  an  inertial  axis,  is 
relatively  unaffected  by  water  particle  velocity  and  vehicle 
maneuvering . 

3.  The  vehicle  is  sufficiently  thin  so  that  there  is  no  dynamic 
pressure  difference  between  the  vehicle  top  and  bottom. 

4.  The  vehicle  length  is  short  compared  to  the  wavelength  of  the 
waves  of  interest. 
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These  assumptions  reduce  the  seaway  disturbances  to  the  vertical  velocity 
of  the  water  particles,  V  ,  which  results  in  the  addition  of  a  force  and 
moment  component  to  equations  (5)  and  (6),  respectively.  Equations  (5) 
and  (6)  become 


mtVX4(t)  -  mXgX3(t)  +  (Zq+mLV)X3(t)  +  ZaX4(t)  it-  Z6eU(t) 

+  (W-B)  -  ZWVW 

JyX3(t)  -  mXgVX4(t)  +  M^X4 (t)  +  (Mq-mXgV)X3(t) 

+  M6e  U(t)  -  WXg  -  MWVW 

and  the  state  variable  model  is  modified  to  yield 
XL(t)  -  -VX2(t)  +  VX4(t) 

X2(t)  -  X3(t) 

x3(t)  -  ¥1X3(t)  +  a2X4(t)  +  bx  U(t)  + 

X4(t)  -  a3X3(t)  +  a4X4(t)  +  b2  U(t)  +  dj 

where 

d i  -  b  3  -  +  “tWw)  VW 

S 

and 

72  -  b"4  -  ^y2^  +  “Xg^w)  VW 

vs 


(9) 


(10) 


(11a) 

(lib) 

(11c) 

(lid) 


(He) 


(Ilf) 
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Comparing  equations  (8)  and  (11)  shows  that  if  the  vertical  water  particle 
velocity  is  constant,  the  effect  of  the  seaway  disturbances  is  incorporated 
into  the  state  veriable  model  as  a  constant  bias. 

The  terms  d^  and  d^  in  equations  (11c)  and  (lid)  are  known  constant 
biases  due  to  the  weight-buoyancy  imbalance  of  the  underwater  vehicle,  the 
different  centers  of  gravity  and  buoyancy,  and  the  seaway  disturbance 
(reference  2).  Equation  (11)  shows  the  steady  state  solution  to  be  given  by 

X2(t)  -  X4(t)  (12) 

X3(t)  -  0 

a2  X4(t)  +  bx  U(t)  -  -d-L  (13) 

*4  X4(t)  +  b2  U(t)  -  -d2.  (14) 


From  equations  (13)  and  (14)  it  is  seen  that  the  state,  angle  of  attack, 
and  input  control,  U(t) ,  have  non-zero  steady  state  values.  Equation 
(12)  shows  that  to  maintain  the  vehicle  at  a  constant  depth,  the  pitch  and 
angle  of  attack  must  be  equal.  More  will  be  said  about  the  effect  of  the 
known  constant  biases  on  the  controller  design  in  Section  III. 

The  state  variable  representation  may  be  written  in  matrix  form  as 


(15) 
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Equation  (15)  is  written  in  a  more  compact  form  by  letting 


0 

-V 

0 

V 

m  m 

0 

XL(t) 

m  “ 

0 

0 

0 

1 

0 

* 

0 

,  x<t)  * 

X2(t) 

0 

•  E  * 

,  ±  * 

0 

0 

T1 

*2 

h 

• 

X3(t) 

dl 

0 

0 

a3 

*4 

b2 

X4(t) 

d2 

Using  the  above  definition,  the  state  variable  model  becomes 
X(t)  -  AX(t)  +  b  U(t)  +  d. 


SIMPLIFIED  STATE  VARIABLE  MODEL 

The  initial  model  to  be  used  in  developing  the  optimal  controller 
assumes  a  neutrally  buoyant  vehicle  with  coincident  centers  of  gravity 
and  buoyancy  and  no  seaway  disturbances.  With  these  assumptions,  equation 
set  (11)  reduces  to 


XL(t)  -  -VX2(t)  +  VX4(t) 

X2(t)  -  X3(t) 

X3(t)  -  a1X3(t)  +  a2X4(t)  +  bjD(t) 
X4(t)  ■  a3X3(t)  +  a4X4(t)  +  b2U (t) 

*  (j_)  indicates  a  vector  quantity 


(17a) 

(17b) 
(17c) 
(17  d) 
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where 


a2  * 


*L 

J„ 


V°L ! 


mtV 


Za 

a4  “  — 
4  114.V 


M, 


5e 


b2  -  2<5e 

04.  V  • 

Equation  set  (17)  may  be  written  as 
X(t)  -  AX(t)  +  bU(t) 


(18) 


where 


A 


»  • 

*  “ 

0  -V  0  V 

0 

0010 

,  1  “ 

0 

0  0  a^  a2 

bl 

0  0  a3  a* 

b2 

. 

L  J 
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Once  the  optimal  control  law  is  formulated  for  the  simplified  state 
model  given  by  equation  (18)  ,  the  control  scheme  will  be  modified  to 
include  the  constant  bias  in  equation  set  (11) . 

Since  the  states  X^(t),  X2(t),  and  X^(t)  are  available  for  measure¬ 
ment,  the  measurement  equation  is  given  by 

M(t)  -  CX(t)  +  v(t) 


where 


m^(t) 

1 

0 

0 

0 

Vj(t) 

M(t)  = 

m2(t) 

C  = 

0 

1 

0 

0 

and  v(t)  =* 

v2(t) 

m3(t) 

0 

0 

1 

0 

v3(t) 

■  — 

m 

It  will  be  assumed  in  this  report  that  the  measurement  noise,  v(t) ,  is 
negligible.  Thus,  the  measurement  equation  becomes 

M(t)  -  CX( t ) .  (19) 

A  signal  flow  diagram  of  the  state  variable  model  given  by  equations  (18) 
and  (19)  is  shown  in  figure  2. 


Figure  2.  Signal  Flow  Graph  of  the  State  Variable  Model 
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The  controllability  and  observability  of  the  state  variable  model  given 
by  equations  (18)  and  (19)  are  shown  in  appendix  B. 

Thus-  far,  a  state  variable  model  of  the  pitch  plane  dynamics  of  an 
underwater  vehicle  has  been  developed.  This  model  was  developed  for  a 
non-neutrally  buoyant  vehicle  with  different  centers  of  gravity  and 
buoyancy.  Also  accounted  for  in  the  model  were  the  effects  of  seaway 
disturbances  on  the  vehicle.  This  model  was  used  to  develop  a  simplified 
model,  with  assumptions  being  made  that  the  vehicle  is  neutrally  buoyant, 
has  centers  of  gravity  and  buoyancy  that  coincide,  and  that  there  are 
no  seaway  disturbances  to  contend  with.  In  the  next  section,  an  optimal 
control  law  is  developed  for  both  of  these  state  variable  models.  Also 
included  in  the  next  section  is  the  development  of  a  Luenberger  observer 
to  make  accessible  the  state  component  angle  of  attack,  so  that  it  can 
be  used  in  the  formulation  of  the  optimal  control  law. 


15/16 

(Reverse  Blank) 
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SECTION  III 

DEVELOPMENT  OF  THE  CLOSED-LOOP  CONTROL  SYSTEM 

In  this  section,  the  optimal  control  law  is  formulated  for  the 
simplified  state  variable  model  given  by  equations  (18)  and  (19).  A 
reduced-order  Luenberger  observer  is  also  developed  to  obtain  an  estimate 
of  the  angle  of  attack.  Once  the  control  system  is  found  for  the  simpli¬ 
fied  state  variable  model,  it  is  modified  to  include  the  addition  of  the 
constant  bias  for  the  more-general  state  variable  model  given  by  equation 
set  (11). 

FORMULATION  OF  THE  OPTIMAL  CONTROLLER:  TRACKING  PROBLEM 

The  optimal  control  law  is  found  from  the  solution  to  the  tracking 
problem.  This  solution  leads  to  an  optimal  feedback  system  with  the 
property  that  the  output  of  the  state  variable  model  is  kept  near  some 
constant  while  simultaneously  minimizing  a  specified  performance  index. 
Following  the  precedure  given  in  reference  1,  a  steady  state  solution  to 
the  tracking  problem  is  found  by  redefining  the  state  variable  model  so 
that  the  techniques  employed  in  the  solution  to  the  output  regulator 
problem  are  applicable.  After  formulating  the  optimal  control  law  for 
the  redefined  model,  the  control  law  can  be  modified  for  the  tracking 
problem. 

The  simplified  linear  state  model  of  the  pitch  plane  dynamics,  from 
equation  (18),  is 

X(t)  -  AX(t)  +  bU(t) .  (20) 


17 


TR  5440 


An  output  state  equation  that  yields  vehicle  depth  is  given  by 
y(t)  =  c’X(t) 

where 

* 

c_'  =  [  1  0  0  0  ]  and  y(t)  =  vehicle  depth. 

Defining  a  new  set  of  state  variables  by  lettine 

x(t)  -  xct)-^ 
y (t)  -  y(t)-zd 


(21) 


(22a) 

(22b) 


and 

U(t)  -  U(t)-Ud  (22c) 

results  in  a  redefined  state  variable  model  of  the  form 

X(t)  -  AX(c)  +  bU(t)  (23) 

y(t)  -_c'i(t).  (24) 

The  terms  Zd  and  Ud  in  equation  (22)  are: 

=  desired  constant  equilibrium  state 

Zd  =  desired  constant  output 

Ud  =  desired  constant  input. 

More  will  be  said  about  the  above  terms  later. 


(')'  indicates  the  transpose  of  (') 
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The  objective  of  the  output  regulator  problem  is  to  determine  a  control 
law  by  minimization  of  a  quadratic  performance  index  such  that  the  out¬ 
put,  y(t) ,  remains  near  zero. 

The  performance  index  for  the  output  regulator  problem  is  defined  as 

CXI 

J  -  -  J  [y * (t)Qy(t)  +  U' (t)RU(t) ]  dt. 

2  0 

Reference  9  derives  the  optimal  control  lax*  for  the  output  regulator 
problem,  and  the  result  is 

U(t)  «  -R-1b_’FX(t)  »  -KX(t)  (233) 

where  the  gains  K  are 

(25b) 


(25c) 

is  the  steady  state  solution  to  the  matrix  Riccati  equation 

FA  +  A’F  -  FbR_1b'F  +  £Qc'  =  0  (26) 

Substituting  equation  (25a)  into  equation  (23)  yields  an  optimal 


K  »  Ck-l  k-2  k3  M 
and  the  matrix  F, 


fll 

«12 

f13 

^14 

f  12 

f22 

f23 

f24 

f13 

f23 

f  33 

f34 

f14 

f24 

f  34 

f  44 

(27) 


closed-loop  system  of  the  form 
X(t)  -  (A-bK)X(t). 
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Even  though  the  matrix  A  does  not  have  to  be  stable,  the  system  given  by 
equation  (27)  is  homogeneous  and  strictly  stable,  therefore 

lim  X(t)  -  0  (28a) 

t-*° 

and  from  equation  (24)  it  follows  that 

lim  y(t)  -  0.  (28b) 

t-M» 

Also,  from  (25a) 

lim  U(t)  =«  0.  (28c) 

t-*-» 


Substituting  the  original  system  state  variables  given  by  equation  set 
(22)  into  equation  set  (28)  shows  that 


lim  X(t)  »  Xd 
t-**> 


(29a) 


lim  y (t)  -  Zd 
t-«° 


(29b) 


and 


lim  U(t) 
t*** 


U 


d- 


Condition  (29b)  implies  that,  in  the  steady  state, 


(29c) 


c’Xd-Zd.  (30) 

Equation  (30)  is  possible  only  if  the  optimal  control  law  contains  a 

constant  bias,  U,,  such  that  in  the  steady  state 
a 

AX,  +  bU,  =*  0  (31) 

— u  ~  a 

This  is  seen  from  equation  (23)  in  the  limit  as  t-*™. 
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Once  equations  (30)  and  (31)  are  formulated,  the  tracking  problem  can 

be  solved  as  an  output  regulator  problem  by  finding  the  control  law  that 

drives  the  output  y(t)  to  zero.  Using  equation  (25a)  for  U(t)  while 

substituting  the  original  state  variables  for  the  states  X(t)  and  y(t) 

into  equations  (23)  and  (24)  shows  that  by  maintaining  the  output  y(t) 

near  zero  for  the  redefined  state  model,  the  output  y(t)  for  the  original 

state  model  is  kept  near  the  desired  outnut ,  Z  ,. 

d 

The  optimal  closed-loop  system  for  the  original  «tate  model  becomes 
X(t)  -  [A  -  bK][X(t)  -  Xj]  (32) 

y (t)  =  c'X(t)  (33) 

The  optimal  control  law  for  the  original  system  is  found  by  solving  for 
the  original  control  U(t)  in  equation  (22c)  while  substituting  the  optimal 
control  law  given  by  equation  (25a)  for  U(t). 

The  optimal  control  law  becomes 

U(t)  =  -K[X(t)  -  X^]  +  Ud  =  -UQ(t)  +  Ud  (34) 

A  closed-loop  structure  of  the  optimal  control  scheme  is  shown  in  figure 
3,  where  double  lines  indicate  the  flow  of  vector  quantities. 

The  optimal  system  can  be  represented  in  a  different  configuration  by 
substituting  from  equation  (31) 

*  ~kud 

into  equation  (32) .  The  closed-loop  system  becomes 


X(t)  -  [A-bK]X(t)  +b[Ud+KXd]. 
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Let 


rd  -  D«r+BCd,  (35) 

then  the  closed-loop  system  has  the  form 
X(t)  =*  [A-bK]X(t)+brd  . 

The  term  rd  is  the  desired  reference  signal  for  the  optimal  closed-loop 
system.  The  alternate  configuration  is  shown  in  figure  4. 

To  put  the  tracking  problem  into  a  form  equivalent  to  an  output  regulator 
problem,  the  desired  output,  Zd,  the  equilibrium  states,  and  the  biased 
input,  Ud,  must  be  determined  for  the  state  variable  model  given  by  equations 
(20)  and  (21).  Using  equation  (30),  the  desired  output  is 

Xid  *  Zd  •  06) 

From  equation  (31)  together  with  the  definition  for  A  and  b  given  by  aquation 
(18) ,  the  equilibrium  point  and  the  biased  input  are  given  by 


-vx2d+vx4d  -  0 
x3d  -  0 

(37) 

alx3d+a2x4d  “  -blud 
a3x3d+a4x4d  *  -b2Ud  * 

Equations  (36)  and  (37)  can  be  written  as 
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X2d  *  X4d 
a2X4d  *  "falUd 
a4X4d  *  "b2Ud 

x3d  -  0. 


(38) 


Relating  equation  set  (38)  back  to  its  true  representations  shows  that  to 
maintain  the  underwater  vehicle  at  a  constant  depth,  the  vehicle's  depth, 
must  equal  the  desired  depth,  Z^;  the  vehicle's  pitch,  X,^,  and 
angle  of  attack,  X^,  must  be  equal;  and  the  vehicle's  pitch  rate,  X^, 
must  be  zero.  Solving  equation  set  (38)  for  the  constant  biased  input, 
U^,  in  terms  of  the  equilibrium  states,  5^,  yields 

Ud  -  0.  (39) 

Thus,  the  equilibrium  states  become 


Substituting  equations  (39)  and  (40)  into  equation  (35)  and  substituting 
for  the  gain  given  by  equation  (25b)  yields 
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klXld 


(41) 


A  block  diagram  of  the  complete  tracking  system  is  shown  in  figure  5. 


Figure  5.  Block  Diagram  of  the  Tracking  System 

REDUCED-ORDER  OBSERVER 

The  states  X^(t),  X^Ct),  and  X^(t)  are  available  for  measurement  at 
the  output,  but  the  state  X^(t),  angle  of  attack,  is  not.  In  order  to 


apply  optimal  control  theory  to  this  system,  all  the  states  must  be 
accessible.  A  reduced-order  Luenberger  observer  (reference  10)  is 
implemented  to  construct  an  estimate  of  the  state  X, (t) . 
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* 

T 

t 


Equations  (18)  and  (19)  are  partioned  in  the  form 


m  1  « 

r-  _ 

Xi(t) 

0  -V  o  V 

1 

X2(t) 

0 

x2(t) 

0  Ol.o 

X2(C) 

0 

. 

* 

+ 

X3(t) 

0  0  ai  1  &2 

x3(t) 

bl 

X4(0_ 

0  a3  |  a4 

X4(t) 

— 

b2 

"l  o  0  1  0  ’ 

^(e)- 

m2(t) 

m 

o 

o 

O 

x2(t) 

m3  (t ) 

o 

rH 

o 

o 

x3(t) 

- 

1  J 

—  -  — ■ 

X4(t) 


Hake  the  substitutions 


mi<t)“ 

X2(r.) 

®2  (t) 

“M(t)“ 

m 

a 

—  —  ^ 

X3(t) 

m3(t) 

Z(t) 

**• 

—  —  — 

m  m 

x4(t) 

Z(t) 

- 

0  -V  0  ’  V 

0  0  1  1  0 

A11 

1  4  1 

1  —12 

1 

0  0  ax  ,  a2 

M 

1 

J* 

1  a22 

00  a3  [  a4 

L  1  J 

and 


(42) 


(43) 


26 


TR  5440 


Equations  (42)  and  (43)  become 


M(t) 

Z(t) 

A11  ' 


b-12 


I-  -  -4-  -  4 

hi  J  k22 


M(t) 

'll' 

J 

Z(t) 

- 

+ 

.b2. 

u(t) 


(44) 


Equation  (44)  is  written  as  a  reduced  plant  and  an  output  equation  of  the  form 
Z(t)  «  A22Z(t)+A2lM(t)+b2U(t)  C^5a) 

M(t)  -  AuM(t)-B1U(t)  -  A12Z(t)  .  (45b) 


The  vector  M(t)  is  available  for  measurement;  if  M(t)  is  differentiated, 
then  M(t)  is  also  available.  Since  U(t)  is  also  measurable,  equation 
(45b)  provides  the  measurement  A^Z^t)  for  the  system  described  by 
equation  (45a),  which  has  state  Z(t)  and  input  A21M(t)  +  b2U(t). 

Equation  (45)  is  written  as 


Z(t)  »  A22Z(t)  +  B  U(t)  (46a) 

M(t)  -  C  Z(t)  (46b) 
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where 

B  -  (A^  b2],  D(t)  - 

and 

I  “  A12  * 

Lee  the  input,  B  U(t),  and  the  output,  H(t) ,  be  used  to  drive  another  system, 
Z(t),  such  that  the  output  of  the  new  system  approximates  the  state  of  the 
original  system,  Z(t).  The  new  system  is  defined  as 

Z(t)  -  F  Z(t)  +  L  CZ(t)  +  TB  U(t)  (47) 

where  T  is  a  transformation  defined  (from  reference  11)  by 
T  A22  -  FT  -  LC  . 

The  error  between  the  real  state  and  the  estimated  state  is  given  by 

Z(t)  -  T  Z(t)  -  Z(t)  -  F[T  Z(t)  -  Z(t)].  (48) 

The  solution  to  the  error  state  equation  becomes 

Z(t)  -  £FT  [T  Z(0)  -  Z(0)]  .  (49) 

If  the  new  system  is  of  the  same  dynamic  order  as  the  real  system,  then  the 
transformation  T  can  become  an  identity  matrix  and  the  new  system 
becomes  an  identity  observer  of  the  form 

Z(t)  -  [A22  -  LC]  Z(t)  +  L  CZ(t)  +  B  U(C).  (50) 


M(t) 

U(t) 


,  M(t)  -  M(t)-A11M(t)-B1U(t) 
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i 


Substituting 

F  »  A22  -  L  £  and  Z  *  Z(t)  -  Z(t) 
into  the  error  state  equation  results  in 

Z(t)  -  [A22  -  L  C]  Z(t).  (51) 

The  eigenvalue  of  equation  (51)  may  be  selected  by  the  proper  selection  of 
the  matrix  L,  which  is  given  by 

t-Ul  l2 

where  1^,  12,  and  I3  are  scalars.  By  being  able  to  control  the  selection  of 
the  eigenvalue  of  (A^  -  L  Cj ,  the  behavior  of  the  error,  Z(t),  can  be 
controlled.  The  solution  to  equation  (51)  is  given  by 

Z(t)  -  eat  Z(0),  '  (52> 


where  a  denotes  the  eigenvalue  of  [A^  -  L  C] .  Equation  (52)  shows  that 
as  long  as  the  eigenvalue  is  negative,  the  error,  Z(t),  will  approach  zero 
regardless  of  the  initial  error,  Z(Q).  The  eigenvalue  of  the  state  estimator 
is  arbitrary,  but  it  must  be  negative  so  that  the  error  will  converge  to  zero. 
Substituting  for  B,  U_(t),  and  C  in  equation  (50)  and  using  equation  (45b), 
an  identity  observer  of  the  form 

Z(t)  -  [A22  -  L  A12]  Z(t)  +  L  M(t)  +  [A21  -  L  Au]M(t) 


(53) 


+  [b2  -  LBX]  0(t) 


is  obtained. 
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The  observer  contains  a  differentiation  of  M(t) .  To  eleiminate  the 

differentiation,  let 

«  •  * 

Z(t)  -  W(t)  +  L  M(t) 

and 

Z(t)  -  W(t)  +  L  M(t).  (54) 

This  yields  a  new  estimator,  eliminating  the  differentiation  of  M(t)  of 
the  form 


W(t)  -  [A22  -  L  A.l2]W(t)  +  {[^2  -  L  AUJ  +  [A22  -  L  Aj^jL} 


(55) 


+  [b2  -  L  BjJ  U(t). 

Figure  6  shows  the  configuration  of  the  reduced  state  estimator. 

The  eigenvalue  of  the  observer  is  arbitrary.  Once  the  system  is  modeled 
on  the  computer,  the  eigenvalue  can  be  adjusted  sc  that  a  good  estimate  of  the 
state  will  be  obtained.  Let  the  eigenvalue  of  the  estimator  be  -a  (a> 0). 

Then,  from  equation  (51),  the  characteristic  equation  is 

X-a4+l1V+l3a2  -  0  .  (56) 

Since  the  estimator  has  an  eigenvalue  at  -o ,  the  characteristic  equation  is 
X+c  -  0.  (57) 

Matching  the  coefficients  of  equations  (56)  and  (57)  yields 

llV+l3a2  ■  o+a4  .  (58) 
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5.(0) 


Figure  6.  Reduced-order  Luenberger  Observer 
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The  element  1^  does  not  enter  into  the  selection  of  the  eigenvalue. 
Therefore,  it  is  arbitrary  and  can  be  set  to  zero.  If  1  is  selected  to 
be  zero,  then,  from  equation  (58)  ,  the  element  1^  becomes 
a  +  a. 


(59) 


Equation  (55)  is  now  given  by 

Wx(t)  -  -oWx(t)  +  [a3  -  (q+a4)  ]  «3(t) 

a2 

bi (a+a*) 

+  [b2  -  - -  ]U(t). 

a2 

To  obtain  an  estimate  of  angle  of  attack,  W^(t)  is  solved  for  in 
equation  (59)  and  substituted  into  equation  (54) . 

Selecting  1^  to  be  zero  and  solving  for  the  element  1^  in  equation 
(58)  results  in  a  new  estimator  of  the  form 


•  a(a+a*) 

W2(t)  -  -aW2(t)  -  - mi(t)  +  (o-+-a4)m2(t) 


(60) 


+  a3m3(t)+b2U(t) , 


Using  equations  (54)  and  (60),  an  estimate  of  the  angle  of  attack,  X^(t), 
is  obtained.  Even  though  two  different  equations  are  used  as  an  estimator 
(other  estimators  may  be  formulated  containing  both  1^  and  1^  elements) , 
the  error  state  equation  (equation  (51))  and  the  transfer  functions  of  the 
estimators  are  identical.  Because  the  estimator  given  by  equation  (60) 
uses  three  measurements,  it  is  more  susceptible  to  modeling  errors, 
since  any  error  in  the  signal  m^(t)  is  integrated  into  the  m0(t)  signal 
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and  the  net  effect  is  a  larger  error.  Therefore,  equation  (59)  is  used 
in  lieu  of  equation  (6)  as  the  estimator. 

The  term  Z(t)  in  equation  (54)  provides  an  estimate  of  the  angle  of 

attack,  X^(t).  If  the  observer  is  selected  properly,  the  transfer 
X4(t)  Z(t) 

functions  ■  and  — .  r  are  the  same.  Appendix  D  demonstrates  that  the 

two  transfer  functions  are  equal.  A  block  diagram  of  the  complete  system 


Figure  7.  Block  Diagram  of  the  Complete  Closed-loop  Control  Scheme 

33 


TR  5440 


MODIFIED  SOLUTION  TO  THE  TRACKING  PROBLEM 

The  optimal  control  law  was  formulated  for  an  underwater  vehicle 
that  was  neutrally  buoyant  with  coincident  centers  of  gravity  and  buoyancy. 
A  modified  optimal  control  law  is  developed  in  this  section  for  the 
general  state  variable  model  descirbed  by  equation  (16),  and  is  repeated 
here  as 

X(t)  =  AX(t)  +  b  U(t)  +  d  (61) 

The  constant  bias,  d^,  is  included  in  the  state  variable  model  as  a 
constant  step.  Equations  (13)  and  (14)  reveal  that  the  angle  of  attack, 
state,  and  the  input  control,  U(t),  have  non-zero  steady  state  values 
due  to  the  constant  bias,  and  equation  (12)  shows  that  the  pitch  and 
angle  of  attack  states  must  be  equal.  Inclusion  of  the  constant  bias 
into  the  solution  of  the  tracking  problem  is  accomplished  by  the  alter¬ 
ation  of  the  desired  equilibrium  states,  X^,  and  the  desired  constant 
biased  input,  U^,  in  equation  (31)  .  The  relationship  between  the 
equilibrium  states  and  the  constant  biased  input  becomes 

AX ,  +  b  U  =  -d  .  (62) 

— a  —  a  — 


Using  equations  (30)  and  (62),  the  equilibrium  states  and  the  biased 
input  are  given  by 


Xld  "  2d 


-vx2d  +  vx4d  -  0 


x3d  -  0 
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al  x3d  +  a2  x4d  “  “bl  Ud  "  dl 
a3  x3d  +  a4  x4d  *  _b2  ud  *  d2  * 

The  equilibrium  states  and  the  biased  input  can  be  written  in  terms  of  the 
constant  coefficients  as 


"xld' 

"Zd 

bl  d2  ~  b2  dl 

Xo  J 

—  -  _  _ 

ZQ 

b2  a2  -  b-^  a^ 

X3d 

0 

_x4d_ 

_X2d 

and 

Ud  -  dj.  a4  ~  dl  a2 
b2  a2  "  bl  a4  • 


(63) 


(64) 


Substituting  equations  (63)  and  <"64)  into  equation  (35)  results  in  a  desired 
reference  signal  of  the  form 


rd  * 


dj  *4  -  d2  a2  +  Kl  Xld  +  K2 
b2  a2  “  bl  a4 


bj  d2  -  b2  dx  +  bj  d2  -  b2  d1 
b2  a2  -  b^  b2  a2  -  b^  a^  . 


(65) 


A  block  diagram  of  the  modified  tracking  system  is  shown  in  figure  8. 
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Figure  8.  Block  Diagram  of  the  Modified  Tracking  System 


From  equation  (63)  ,  it  is  seen  that  to  maintain  an  underwater  vehicle  at 
a  constant  depth,  the  desired  pitch  and  angle  of  attack  states  are  no 
longer  zero.  In  order  for  the  states  to  have  non-zero  values,  the 
desired  input,  U^,  must  have  a  non-zero  value  as  shown  by  equation  (64). 

MODIFIFI CATIONS  TO  THE  REDUCED-ORDER  OBSERVER 

The  reduced-order  observer  must  be  altered  to  include  the  addition 
of  the  constant  bias  in  the  state  variable  model  given  by  equation  (61) . 
Using  the  same  techniques  in  developing  the  reduced-order  observer  in 
Section  II,  the  modified  observer  will  be  constructed  in  this  section. 
Partitioning  the  state  variable  model  in  the  form 
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and 


M(t)  -  C  Z(t) 


(68) 


where 

D  *  [A21  b2  b4] ,  U(t)  » 

M(t)  =■  M(t)  -  Au  M(t)  -  U(t)  -  B2  and  C  *  I12  . 

Substituting  equations  (67)  and  (68)  into  equation  (50)  of  an  identity 
observer  results  in  a  modified  reduced-order  observer  of  the  form 

Z(C)  »  U2Z  -LC]  2(t)  +  L  C  Z(t)  -6  Z>  U( t).  (69) 

Substituting  for  D,  £(t) ,  M(t)  and  £  in  equation  (69)  yields 
*  _  _  »  ^  _ 

Z(t)  -  [A22  -  LC]  Z(t)  +  L[M(t)  -  Au  M(t)  -  Bjl  U(t)  -  B^] 

+  A21  M(t)  +  b2  U(t)  +  b4  . 

To  eliminate  the  differentiation  let 
T(t)  -  W(t)  +  L  M(t) 

and 

I(t)  -  W(t)  +  L  M(t) 


H(t) 
U(t)  , 
1 
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which  results  in  a  new  estimator  of  the  form 


W(t)  -  [A22  -  L  C]  W(t)  +  {[A22  “  L  Aj_2 +  [ a21  “  L  Alin  M(t) 


+  [b2  -  L  3j_ ]  U(t)  +  b4  -  L  I2  • 


(70) 


The  matrix  of  the  observer  is  found  in  the  same  manner  as  in  the 
previous  section  and  is  given  by 


L  -  [0  0  q+a4  ]. 

a2 

Substituting  for  L  in  equation  (70)  results  in  a  modified  observer  of  the 
form 


— .  .  _  _  aWS'+ai)  -  0(0+84) 

W(t)  -  -  o  w(t)  +  fa3  -  -L-. -  — -  ]  M3(t) 


+  tb,  -  bi(^)  1  «t>  *  b,  -  bj^l 


(71) 
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SECTION  IV 

RESULTS  OF  LABORATORY  RUNS 

The  closed-loop  control  scheme  for  the  underwater  vehicle  shown  in 
figure  7  was  simulated  on  a  digital  computer  using  the  hydrodynamic 
coefficients  of  a  typical  torpedo  traveling  at  a  constant  speed.  The 
state  transition  matrix  developed  in  appendix  C  was  used  to  solve  the 
state  equations}  and  the  steady  state  solution  to  the  matrix  Riccati 
equation  was  found  using  a  standard  numerical  integration  technique. 
Included  in  the  control  scheme  were  limitations  of  30  degrees  for 
vehicle  pitch  and  rudder  deflection.  Plots  were  made  of  the  torpedo's 
depth,  pitch,  pitch  rate,  angle  of  attack,  and  rudder  angle  (input 
control).  Since  a  Luenberger  observer  was  used  to  estimate  the  angle 
of  attack,  a  plot  of  the  estimator  error  was  also  included.  The 
weighting  terms  in  the  performance  index  were  varied  to  achieve  the 
best  performance  for  the  torpedo;  i.e.,  minimum  time  to  reach  the 
desired  depth.  Table  1  contains  a  summary  of  all  the  torpedo 
trajectories. 

In  the  first  set  of  plots,  the  torpedo  was  initially  at  50  meters, 
diving  to  a  desired  depth  of  100  meters  at  a  constant  speed  of  25 
meters/sec.  Figure  9a  shows  that  the  torpedo  arrives  at  the  desired 
depth  at  approximately  5  seconds;  vehicle  pitch,  pitch  rate,  angle  of 
attack,  and  rudder  angle  approach  the  desired  values  as  shown  in 
figures  9b,  9c,  9d,  and  9f.  The  plot  of  the  observer  error,  figure 
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9e,  shows  that  there  is  a  small  initial  error  during  the  first  5 
seconds  of  the  run  until  the  angle  of  attack  approaches  its  steady 
state  value  (figure  9d).  At  this  time,  the  observer  error  goes  to 
zero.  In  this  set  of  plots,  the  initial  conditions  on  the  observer 
and  the  angle  of  attack  are  identical.  In  the  next  set  of  plots, 
figure  10,  there  is  a  10-degree  difference  between  the  initial 
conditions.  There  is  no  significant  change  between  the  plots  of 
figure  9  and  figure  10,  except  in  the  plot  of  the  angle  of  attack 
error,  figure  lOe.  Because  of  the  large  vertical  scale  and  small 
observer  error,  the  observer  error  appears  to  be  zero.  However,  there 
is  an  initial  observer  error  of  10  degrees  which  approaches  zero 
rapidly  because  of  the  large  observer  pole.  Changing  the  observer 
eigenvalue  to  a  smaller  value  and  rerunning  the  trajectory  increases 
the  time  for  the  observer  error  to  approach  zero  (see  figure  11). 

This  time  increase  also  has  a  small  effect  on  the  angle  of  attack, 
which  is  seen  by  comparing  figures  lOd  and  lid.  The  angle  of  attack 
takes  on  different  initial  values  until  the  torpedo  reaches  its 
desired  depth.  There  are  also  small  changes  in  the  other  states,  but 
these  changes  cannot  be  detected  in  the  plots.  The  trajectory  of 
figure  9  was  rerun  using  the  actual  angle  of  attack  in  place  of  the 
estimated  value  in  determining  the  optimal  control  law.  Figure  12 
shows  that  the  change  in  the  performance  of  the  controller  is 
insignificant . 
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In  all  of  the  trajectories  that  were  simulated  (figures  9  through 
12),  the  pitch  angle  and  the  rudder  deflection  angle  reached  their 
saturation  points.  By  changing  the  weighting  terms  in  the  performance 
index,  the  amount  of  deflection  can  be  altered.  The  weighting  term 
which  regulates  deviations  in  vehicle  depth  was  reduced,  and  a  plot  of 
this  trajectory  is  shown  in  figure  13.  From  this  plot,  it  is  seen 
that  the  pitch  angle  and  the  rudder  deflection  angle  do  not  reach  the 
saturation  levels,  but  comparing  figures  9a  and  13a  shows  that  the  time 
for  the  torpedo  to  reach  its  desired  depth  has  been  increased.  Increasing 
the  weighting  on  the  vehicle  depth  error  has  a  reverse  effect  and  can 
cause  the  torpedo  to  oscillate  about  the  desired  trajectory,  as  can  be 
seen  in  figure  14,  which  shows  that  the  states  overshoot  their  desired 
values  and  oscillate  about  them  while  approaching  the  desired  values. 

The  speed  of  the  vehicle  has  an  effect  on  the  performance  of  the 
controller.  Even  though  the  speed  could  have  been  normalized  out  of 
the  state  variable  model  (reference  2) ,  an  optimal  control  law  can  be 
found  for  various  vehicle  speeds.  The  trajectory  of  figure  9  was  run 
with  a  50  percent  reduction  in  torpedo  speed.  The  plots  for  this  case 
are  shown  in  figure  15.  Comparing  figures  9  and  15,  it  is  seen  that 
the  plots  are  similar,  but  the  torpedo  takes  longer  to  reach  the 
desired  depth  in  figure  15a,  as  would  be  expected.  Increasing  the 
weighting  on  the  depth  error,  figure  16,  provides  little  improvement 
on  the  performance  of  the  torpedo. 
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The  non-neutrally  buoyant  vehicle  with  different  centers  of 
gravity  and  buoyancy  was  also  simulated.  The  trajectory  of  figure  15 
was  used  and  the  plots  are  shown  in  figure  17.  In  this  set  of  plots, 
the  desired  trajectories  of  the  pitch  and  angle  of  attack  take  on 
non-zero  values  in  order  to  keep  the  torpedo  from  sinking.  Once  the 
torpedo  reaches  the  des.  ed  depth,  the  pitch  and  angle  of  attack 
approach  the  desired  trajectories.  The  steady  state  values  for  the 
pitch  and  angle  of  attack  are  small  (see  table  1),  but  the  amount  of 
rudder  deflection  necessary  to  maintain  these  values  is  large,  as  can 
be  seen  from  figure  17f. 

When  an  underwater  vehicle  operates  near  the  surface  in  a  high  sea 
state  condition,  ocean  currents  may  cause  the  vehicle  to  deviate  from 
its  desired  trajectory.  These  seaway  disturbances  were  incorporated 
into  the  closed-loop  tracking  scheme  as  a  known  constant  bias  caused 
by  the  vertical  velocity  of  the  water  particles.  Figure  18 
demonstrates  the  performance  of  the  controller  for  a  torpedo  operating 
under  these  conditions.  The  torpedo  is  at  an  initial  depth  of  50 
meters  climbing  to  a  desired  depth  of  2  meters.  It  is  assumed  that 
the  seaway  disturbance  had  a  constant  effect  on  the  torpedo  throughout 
the  trajectory  with  a  vertical  velocity  of  1  meter/sec.  From  the 
plots,  it  is  seen  that  the  states  approach  their  desired  values. 

Since  the  pitch  and  angle  of  attack  no  longer  have  zero  steady  state 
values,  the  rudder  deflection  angle  (input  control)  also  takes  on  a 
non-zero  steady  state  value. 
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Comparing  figures  17  and  18,  it  is  interesting  to  note  that  the 

desired  inputs  are  the  same,  but  there  is  a  significant  difference 

between  the  steady  state  values  of  the  pitch  and  angle  of  attack.  By 

examining  equation  (64)  and  substituting  for  the  terms  d^  and  d^ 

from  equations  (lie)  and  (Ilf)  and  the  terms  a„  and  a,  from 

2  4 

equations  (8f)  and  (9h),  the  desired  input  becomes 


b;ja4_(I“XgJyZwZQt+mtiaXgMwMa)Vw  _  b4a2+(mXgJyZwZa+mtmXgMwMa)Vu 


V8‘ 


ve- 


^2a2  ~  ^la4 


which  reduces  to 

U,  -  b3a4”b4a2 

a  ^ _j~ 
b2a2"bia4 

and  is  independent  of  the  vertical  velocity  of  the  water. 

The  simulations  have  shown  that  the  linear  mean-square  optimal 
control  law  is  a  favorable  approach  in  the  control  of  an  underwater 
vehicle.  By  changing  the  weighting  terms  in  the  performance  index, 
desirable  trajectories  of  various  underwater  vehicles  can  be  achieved 
over  a  wide  range  of  speeds.  For  the  example  shown  it  was  found  that 
the  weighting  terms  used  in  figure  9  yielded  a  satisfactory 
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performance  of  the  controller,  and  that  an  observer  eigenvalue  that 
was  of  slightly  greater  magnitude  than  the  eigenvalues  of  the 
closed-loop  system  achieved  the  best  results.  It  was  also  found  that 
there  was  no  need  to  change  the  performance  index  weighting  terms  when 
there  was  a  50  percent  reduction  in  speed. 


Table  1.  Summary  of  Torpedo  Trajectories 


Observer 

Initial 

Observer  Conditions 

Eigenvalues  (deg) 


Closed-loop  Control  Scheme 
Eigenvalues 


-6.98  -28.95  6 

-6.98  -28.95  -.6*j.6 

-6.98  -28.95  -.o*j.6 

-6.98  -28.95  -.6*]. 6 

-0.98  -28.95  - . 287*  j .289 

-7.23  -28.95  -2. 7* j3 

-3.49  -14.48  - .3* j .3 

-3.49  -16.  -*8  -.-»5*j.45 

-3.37  -14.54  - .3l*j .31 

-3.37  -14.56  - . 31* j • 31 


The  state  X^Ct)  is  used  in  determining  the  optimal  control  law 


Initial  Conditions  on  Staces 


Xj ( 0  >  X2(0)  X3(0)  X4(0) 

(m)  (deg)  (deg /see)  (deg) 


Desired  Values 


x2d  '3d 

(deg)  (deg/sec) 


*  The  state  X,(t)  is  used  in  determining  the  optimal  control  law 
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Figure  11.  Optimal  Trajectories  with  Changes  in  Observer 
Initial  Conditions  and  Eigenvalue 
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SECTION  V 

SUMMARY,  CONCLUSIONS,  RECOMMENDATIONS 

This  report  has  investigated  the  feasibility  of  using  modern 
control  theory  and  Luenberger  observer  theory  in  the  development  of  a 
general  controller  that  is  easily  adaptable  to  various  underwater 
vehicles.  The  closed-loop  control  scheme  was  simulated  on  a  digital 
computer  using  the  hydrodynamic  force  and  moment  coefficients  of  a 
typical  torpedo.  Included  in  the  simulations  were  limitations  on 
torpedo  pitch  and  rudder  deflection.  The  simulations  demonstrated 
that  the  control  scheme  performed  sat isf ac torily  for  the  neutrally 
buoyant  model,  the  non-neutrally  buoyant  model,  and  when  the  torpedo 
was  under  the  influence  of  constant  seaway  disturbances.  The  effects 
that  the  weighting  terms  in  the  performance  index  have  on  the  torpedo 
trajectory  were  also  shown  in  the  simulations.  By  varying  these 
terms,  suitable  torpedo  performances  were  obtained.  Changes  in 
simulated  torpedo  speed  were  shown  to  have  insignificant  effects  on 
the  performance  of  the  control  scheme. 

The  control  law  developed  in  this  report  maintains  an  underwater 
vehicle  on  a  constant  path.  In  some  instances  it  may  be  necessary  for  a 
torpedo  to  travel  on  a  time-varying  trajectory  to  a  target.  The 
present  closed-loop  tracking  scheme  needs  to  be  modified  to  follow  a 
time-varying  reference  input.  It  was  also  found  that  the  underwater 
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vehicle  can  be  controlled  without  feeding  back  all  the  states.  An 
investigation  into  finding  a  suboptimal  control  law  should  be  made  and 
the  results  of  this  investigation  compared  against  the  performance  of  the 
optimal  control  scheme. 
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APPENDIX  A 
GLOSSARY  OF  TERMS 


Vehicle  Constants 
m  Vehicle  mass 

V  Vehicle  velocity 

D  Vehicle  diameter 

W  Vehicle  weight 

B  Buoyancy 

CB  Center  of  buoyancy 
L  Vehicle  length 

CG  Center  of  gravity 
A  Cross-sectional  area 

Xg  Distance  from  CB  to  CG  on  X-axis 

m^  Longitudinal  mass 
Transverse  mass 

Jy  Moment  of  inertia  about  y-axis  due  to 
vehicle  mass 

Vw  Vertical  velocity  of  the  water  particles 


Kg 

meters/sec 

meters 

newtons 

newtons 

meters 

2 

meters4 

meters 

Kg 

Kg 

Kg-meter-^ 

meters/sec 


Hydrodynamic  Coefficients 
Force  Coefficients 


Zq  Derivative  of  normal  force  component  with  respect 
to  angular  velocity  component,  q 

Za  Derivative  of  normal  force  component  with  respect 
to  angle  of  attack,  a 
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Zy  Derivative  of  normal  force  component  with  respect 
to  velocity  component,  w 

Zje  Derivative  of  normal  force  component  with  respect 
to  elevator  angle  component,  <Se 


Moment  Coefficients 


Mq  Derivative  of  moment  component  with  respect  to 
angular  velocity  component,  q 

Ma  Derivative  of  moment  component  with  respect  to 
angle  of  attack,  a 

Mw  Derivative  of  moment  component  with  respect  to 
vehicle  velocity  component,  w 

Mjje  Derivative  of  moment  component  with  respect  to 
elevator  angle  component,  <5e 
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APPENDIX  B 

OBSERVABILITY  AND  CONTROLLABILITY  OF  WEAPON  MODEL 

Application  of  a  Luenberger  observer  and  optimal  control  theory  to 
a  time-invariant  system  described  by  the  state  equation 

X(t)  *  AX(t)+bU(t)  (B1) 

M(t)  -  CX(t) 

(B.2) 

requires  that  the  system  be  observable  (reference  7)  and  controllable 
(reference  1). 

Observability  Test: 


The  rank  of  the  matrix  C  is  three.  The  observability  of  the  fourth-order 
system,  (B.l)  and  (B.2),  may  be  determined  (from  reference  9  )  by 


V.  ,  becomes 
4-3 


j(0  denotes  the  rank  of  the  matrix 
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The  rank  of  is  four;  therefore  the  system  is  observable.  The 

observability  index  for  this  system  is  two. 

Controllability  Test: 

p  (U)  *  P  [b_  Ab_  Ab_  AbJ  =  4 

U  becomes 


where 

*  V[  (b^a2+b2S4)~b^] 

u14  *  (b^aj+b2&2 )  (14"®  j)+a4 (b^a^+bjS^)  ] 

^23  “  b^a-^+b2a2 
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U24  a  al ^albl+a2b2^+a2  ^ia3+^?a4) 

U32  *  b2a2+b2a2”  ^23 

U33  a  al^blal'i'b2'12^+a2^bla3+b2a4) 

2 

u34  *  (b]_a]_+b2a2)  (a]_+a2a3)  +  (bxa3+b2a4)  (aia2+a2a4) 
u42  ”  bla3+b2a4 

U^3  ”  a3 (blal+b2a2 )+a4 (b^a3+b2a^ ) 

2 

U44  -  (a3a3_+a3a4)  (b1a1+b2a2)+(a3a2+a4)  (b2a3+b2a4) . 
Let  U  be  written  in  a  partitioned  form  as 


0 

b2v 

1 

1 

u13 

”14 

0 

_bl . 

l 

•  | 

_U23  _ 

u24 

31 

1 

bl 

u23 

1 

U33 

U34 

C  1  D 

U42 

1 

1 

U43 

”44 

where  A,  B,  C,  and  D  are  2X2  matrices.  The  determinant  can  be  found 

from  the  formula  (reference  10) 

.  (B .  3) 

i U !  -  I  A  -  3  D_1  c I  j  D !  . 

Let  the  | D (  ■  d  then  (B.3)  becomes 

| U |  -  !dA  -  3  D*  C| 

(B.4) 

where  D*  is  the  transpose  of  the  cofactor  of  the  matrix  D. 
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The  determinant  of  the  matrix  D  is  given  by 
d  -  |D|  -  U33  U44  -  U34  U43  , 
and  the  transpose  of  the  matrix  D  becomes 

D*  ■ 


(B  .5) 


U44 

-U34 

-U43 

U33 

(B.6) 


Substituting  equation  (B.5)  and  (B.6)  into  equation  (B.4),  the  determinant 
of  the  matrix  U  is  written  as 


-(u13u44-u14u43)bl 

~<u14u33-u13u34)b2 

-  (U2  3^44-^24^43  ^  bl 

-(U24U33-U23U34)b2 


db2V- (U13U44-U14U4  3)^32 
“(U14U33~U13U34)U42 

I 

dbi~  (U23u44~u24u43^u32 
-(U24U33-’,23D34)”42 


(B.7) 


The  determinant  of  the  matrix  U  becomes 


|U |  -  -( (U13U44-U14U43)b1+(U14U33-U13U34)b2][db1-(U23U44-U24U43)U32 
-(^24U33~U23^34)U42 1  + 

[ (U23u44~u24u43^bl+^u24u33_u23u34^b2^ •  (B.8) 

The  determinant,  equation  (B.8),  cannot  be  evaluated  until  the  elements 
of  the  A  matrix  and  b_  vector  are  defined  for  a  specific  application. 
Therefore,  before  an  optimal  control  law  can  be  formulated  for  the  under¬ 
water  vehicle  state  variable  model,  equation  (B.8)  must  be  evaluated  for 
the  specific  vehicle  constants  and  hydrodynamic  coefficients. 
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APPENDIX  C 

STATE  TRANSITION  MATRIX 


The  seaee  transition  matrix  is  found  by  taking  an  inverse  Laplace 
transform  of  the  resolvent  matrix.  The  resolvent  matrix  for  a  time- 
invariant  system  described  by  the  state  equations 


X(t)  -  AX(t)+BU(t)  (c>1) 

M(t)  -  CX(t)  (c . 2) 

is  given  by 

*(S)  -  [SI-A]-1. 


The  matrix  [SI-A]  is  written  in  a  partitioned  form  as 


[SI-A] 


S  V  1  0  -V 

I 

0  S  I -1  0 

- - 1  -  —  — 

0  0  ,  S-ajL  -a2 

0  0  * -33  S-a^ 


(C.  3) 


where  a,  b,  and  c  are  2><2  matrices  and  $  denotes  a  2x2  null  matrix.  The 
inverse  is  found  by  multiplying  equation  (C . 3)  by  a  matrix  such  that  the 
product  yields  the  identity  matrix.  Let 


"a  '  b‘ 
~  -  h  - 
5  I  c 


"i  1  r 

$  i  i 


(C .  4) 
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where  D,  E,  F,  and  G  are  2x2  matrices  and  I  is  a  2x2  identity  matrix. 
The  resolvent  matrix  is  given  by 


*<S) 


D  1  E 

I - r-  - 

F  I  G 


(C  .5) 


Multiplying  out  equation  (C.4)  yields 


aD+bF 

aE+bG 

1 - 

-e- 

cF 

cG 

M 

- 1 

1  M 

J  _ 

1 

Equating  terms  and  solving  for  D,  E,  F,  and  G  yields 
F  -  $ 


G  53  c 


S-a4  a2 


La3 


S-a 


1  J 


(S-a4) (S-a1)-a2a3 


1/S 

0 


-V/S2 

1/S 


and 


E  *  -a“^  b  c_1 


<S-a4) 


v  _  a3V  a2v  -  (s_al)v 


S7 


-<S-a4) 


S2 


~a2 

S 


[ (S-a4) (S-a^)  -a2  a3 
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From  equation  (C.5), 

the  resolvent  matrix  is 

given  by 

r— 

V(a3-l)S+a4V 

VS2-aiVS-a2V 

l/S  -v/s^ 

S2[(S-W)(S-Z)j 

S2 (S-W) (S-Z) 

0  1/S 

(S-a4) 

a2 

MS)  - 

S[(S-W)(S-Z)] 

S[ (S-W) (S-Z)] 

0  0 

(S-a4) 

a2 

(S-W) (S-Z) 

(S-W) (S-Z) 

0  0 

a3 

(S-a2) 

(S-W) (S-Z) 

(S-W) (S-Z) 

where 

al+a4+  / 

w  - 

1+a4)2  +  4(a2a3~a1a4) 

2 

and 

2  „  al+a4-  A3 

2 

1+a4)  +  4(a2a3-aja4) 

2 

The  inverse  Laplace  transform  of  equation  (C 

6)  yields  the  state 

matrix  of  the  form 

l  -Vt 

Wt  Zt 

A^t+Ao+A^e  +A4£ 

E1t+E2+E3ewt+E4£2t 

Mt)  » 

0  1 

0  0 

31t+B2eWt+B3eZt 

cl£Wt+C2£^C 

r  ►j.r-  Wt,r  „Zt 
Mc+f2£  +F3e 

G1£Wt+G2eZt 

0  0 

D1eWt+D2£Zt 

H1eWt+H2eZt 

(C.  6) 


(C.  7) 


03 
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where  the  coefficients  are  found  by  performing  a  partial  fraction  expansion 
of  the  terms  in  the  resolvent  matrix,  equation  (C.6).  The  partial  fraction 
expansion  of  the  term  ‘*’13 (S )  is 


V(a3-l)S+a4V 

S2 (S-W) (S-Z) 

A1 

S7 

Ao  Ao  A  / 

+  _£  +  3  +  4 

S  S-W  S-Z 

A1  " 

V(a3-l)S+a4V 

-  V 

(S-W) (S-Z) 

s«o 

WZ 

d 

V(a3-l)S+a4V 

- 

(S-W) (S-Z)V(a3-l)-(V(a4-l)S+a4V) [2S-W-Z] 

dS 

(S-W) (S-Z) 

s-o 

(S-W)2(S-Z)2 

Ao  *> 

WZV(a3-l)+a4V(W+Z) 

«- 

W2Z2 

An  m 

V(a3-l)S+a4V 

V(a3-l)W+a4V 

3 

S2(S-Z) 

S-W 

W2(W-Z) 

Aa  * 

V(a3-l)S+a4V 

V(a3-l)Z+a4V 

4 

S2(S-W) 

S-Z 

Z2(Z-W) 

S-0 


The  partial  fraction  expansion  of  the  term  $23^)  is 


(S-a4) 


S(S-W) (S-Z) 


+  J4_  +  _^3_ 

S  S-W  5-Z 
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S-W 


s-z 


_3_ 

Z-W 


The  partial  fraction  of  the  term  $X^(S)  is 


VS  -axVS-a2V 
S2(S~W) (S-Z) 


E1  +  E2  +  E3  +  _^4_ 

s2  s  s-w  S-Z 


EL  -  VS^-a^S-azV  |  ,  -a2V 


(S-W) (S-Z) 


s-o 


wz 


E2  *  _ 

dS 


VS  -a1VS-a2V 
(S-W) (S-Z) 


S-0 


(S-W)  (S-Z)  [2VS-a1V]-[VS‘i-a1VS-a2V]  (2S-W-Z) 
(S-WMS-Z) 


-WZa1V-a2V(W+Z) 

W2Z2 


VS^-a1VS-a2V 

S2(S-Z) 


S-W 


VW2-a1VW-a2V 


W* (W-Z) 


VS^-a1VS-a2V 


S2(S-W) 


S-Z 


VZ^-axVZ-a2V 

Z2(Z-W) 
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a 
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The  partial  fraction  expansion  of  the  term  is 


S-a]_ 

Hi 

_ _  ^ 

(S-W) (S-Z) 

S-W 

S-a-i 

Hi  -  _ i 

W-ax 

s-z  s=w 

W-Z 

S"“3-i 

Ho  *  1 

at  ^ 

2 

T^T  . 

s»z 

C-8 


I 
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APPENDIX  D 

TRANSFER  FUNCTION  OF  THE  OBSERVER 
The  state  variable  model  is  given  by 

V 

0 

a2 
a4 


Xi<t) 

0 

-V 

0 

X2(t) 

0 

0 

1 

x3(t) 

0 

0 

al 

X4(c) 

0 

0 

a3 

^(c) 

"l 

0 

0 

0 

m2(c) 

M 

0 

1 

0 

0 

m2(t) 

0 

0 

1 

0 

X1(t) 

- 

0 

X2(t) 

+ 

0 

x3(t) 

bl 

J 

- 1 

/-s 

iJ 

<*■ 

X 

J 

b2 

x1(t) 
x2(t) 
x3(t) 
x4(t) 

and  the  reduced-order  observer  is  written  as 


U(t) 


(B.l) 


(D.2) 


i(t)  -  -aw<0  ♦  [a3  -  „3(t)  +  (b2  -  ^V)  „(t).  (0.3) 


A  signal  flow  graph  of  the  model  given  by  equations  (D.l),  (D.2),  and 
(0.2)  is  shown  in  figure  D-l.  The  signal  flow  graph  in  figure  D-l  can  be 
simplified  by  calculating  the  open-loop  transfer  functions  from  the 
equation 

G(S)  -  C  MS)b 


D-l 
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where 


C 


1 

0 

0 


0  0  0 
10  0 
0  10 


0 

0 


b  ■ 


b 

b 


1 

2 


and  4>(S)  is  the  resolvent  matrix  in  appendix  C.  Using  the  above  equation, 

b2V  S2  +  (b1a3V-b1V-b2a1V)S  +  bjVa^b^V 
S2(S-W) (S-Z) 


G^  (S) 

G2(S) 

m 

S(S-W) (S-Z) 

g3(s) 

blS“^ia4+^2a2 

(S-W) (S-Z) 


Let 


El  *  G1(S) 


m3(S) 

U(S) 


£2  -  G2(S) 


m2(S) 

U(S) 
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and 


f 


3 


=  G3(S) 


m3(S) 

_U(S)"  =  Sf2* 


then  the  signal  flow  graph  is  modified  to  the  signal  flow  graph  of  figure  2. 


Mason's  Formula  and  figure  2  are  used  to  find  the  transfer  function, 

X4(S) 

U(S)  . 


Loops : 


^  'i 


a2a3 

S2 


and  L 


-  al 
3  — 

S 


A  *  l-(L2+L2+Lg)+L^L-3 


S^-(a4+a1)S-a2a3+a4a1 


Forward  Paths : 


*1 


S-a, 


and 


bla3 

S2 


D-3 


r 
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r 

Transfer  Function: 

I 

X4(S)  G1A1+G2A2 

t)  2  S“*b  2  ^  j+b  ^  ^ 

1 

U(S)  A 

S^- (a4+a2) S-a2a2+a4a^  . 

(D-4) 

Figure  D-2  is  used  to  find 

Z  (S) 

the  transfer  function, 

U(S) 

r 

Loop : 

1 

4  -  -  - 
1  s 

1 

S+c 

A  *  _ 

S 

I 

Forward  Paths : 

b2  b^(a+a4) 

G1  *  —  *  - 

S  a2S 

A^  =»  1 

[ 

G2  ■  0+3  A  f3 

.  S+o 

a?  ■ 

1 

2  a2  3 

s' ' 

I 

and 

ai(a+a4) 

G-j  »  fa^  -  1  4 

a2 

o(a+a4)  }  f3  A3  .  1 

a2  S 

1 

D-4 

■ 

J 

TR  5440 


Transfer  Function: 


b2  ^(o+a^)  0+a4  (S+o)  a-j  -a-j_(a+a^)  -o(o+a^)  j 


2  j 


which  reduces  to 

Z(S)  b2  a2  -  b]_(c+a4)  4-  [aS+a^S-* ja^-a4ai+a3a23  f3 

»  *  _ -  -  -  __  -  -  --  —  -  _  -  ■  -  _  * 

U(S)  a2(S+o) 

Substituting  for  £3  and  cancelling  similar  terms  yields 

Z(S)  a2(S-w)  [b2S-b2a]_+b]_a3l 

U(S)  [S^  -  (a4+a^)S  -  a2a3+a4a^l  a2  (S+cO 

Z(S)  b^S  -  b2£;L+b-j.a3 

U(S)  ^"-"Ta^+a^S-a^a^+a^a^" . 

Comparing  equations  (D.4)  and  (D. 5)  shows  that 


X4(S) 

U(S) 
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